#include "zf_common_headfile.h"

// 二者本质相同
// 右: 正交编码器
#define ENCODER_QUADDEC                 (TIM6_ENCODER)
#define ENCODER_QUADDEC_A               (TIM6_ENCODER_CH1_P20_3)
#define ENCODER_QUADDEC_B               (TIM6_ENCODER_CH2_P20_0)

// 左: 方向编码器
#define ENCODER_DIR                     (TIM2_ENCODER)
#define ENCODER_DIR_PULSE               (TIM2_ENCODER_CH1_P33_7)
#define ENCODER_DIR_DIR                 (TIM2_ENCODER_CH2_P33_6)

// 车轮齿数: 68
// 编码器齿数: 28
// 编码器一圈值: 1024
// 车轮一圈值: 2486.857
// 车轮周长: 0.21

#define DISTANDE_RATIO (11778.0f)  // 编码器值/每米

int16 encoder_data_quaddec = 0;
int16 encoder_data_dir = 0;

void encoder_init (void)
{
    // 两个编码器相同,初始化相同
    encoder_quad_init(ENCODER_QUADDEC, ENCODER_QUADDEC_A, ENCODER_QUADDEC_B);
    encoder_dir_init(ENCODER_DIR, ENCODER_DIR_PULSE, ENCODER_DIR_DIR);
}

float speed_alpha = 0.6;

void encoder_get (void)
{
    // 读取编码器数值,由于左右编码器相同,一左一右摆放故读数相反,左侧乘一个负号保持一致
    motor_l.current_speed_raw = -encoder_get_count(ENCODER_DIR);
    // 计算速度
    motor_l.current_speed = motor_l.current_speed * speed_alpha + motor_l.current_speed_raw * (1 - speed_alpha);
    motor_l.total_encoder += motor_l.current_speed_raw;

    // 读取编码器数值
    motor_r.current_speed_raw = encoder_get_count(ENCODER_QUADDEC);
    // 计算速度
    motor_r.current_speed = motor_r.current_speed * speed_alpha + motor_r.current_speed_raw * (1 - speed_alpha);
    motor_r.total_encoder += motor_r.current_speed_raw;

    // 清理标志位
    encoder_clear_count(ENCODER_QUADDEC);
    encoder_clear_count(ENCODER_DIR);
}

// 获取当前距离
float get_real_distance (void)
{
    float Real_Distance;

    Real_Distance = (float)(((motor_l.total_encoder + motor_r.total_encoder) / 2) / DISTANDE_RATIO);

    return Real_Distance ;
}
